#ifndef LOCAL_NAVIGATOR_HPP
#define LOCAL_NAVIGATOR_HPP

#include <rclcpp/rclcpp.hpp>
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/bool.hpp"
#include "geometry_msgs/msg/twist.hpp" 
#include "local_navigation/center_json.hpp"
#include "local_navigation/vel_json.hpp"
#include <memory>
#include <string>
#include <vector>
#include <functional>



namespace local_navigation{

    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

    // 速度控制进程状态枚举
    enum ControlState {
        IDLE,           // 空闲状态
        SAMPLING_X,     // X轴采样状态  
        MOVING_X,       // X轴移动状态
        SAMPLING_Y,     // Y轴采样状态
        MOVING_Y,       // Y轴移动状态
        VERIFYING,      // 最终验证状态
        REACHED         // 已到达目标状态
    };

    // 样本数据
    struct Point3D {
        double x, y, z;
    };

    class LocalNavigator : public rclcpp_lifecycle::LifecycleNode
    {
        public:

            explicit LocalNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

        private:

            CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
            CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
    
            void declare_parameters();

            // 回调函数
            void centerJsonCallback(const std_msgs::msg::String::SharedPtr msg);
            void controlLoop();

            // 状态处理函数
            void handleIdleState();
            void handleSamplingXState();
            void handleMovingXState();
            void handleSamplingYState();
            void handleMovingYState();
            void handleVerifyingState();
            void handleReachedState();
            
            // 辅助函数
            void startSampling();
            void processSamples();
            std::pair<std::vector<double>, std::vector<double>> removeOutliers(
                const std::vector<double>& x_coords, const std::vector<double>& y_coords);
            void startMovingToTarget();
            void startMovingYToTarget();
            void stopRobot();
            void publishVelocity();
            double calculateMovementTime(double distance, double max_vel, double max_acc);
            void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                        double target_linear_x, double target_linear_y, double target_angular); 
            void publishReachedStatus(bool success);
            void resetvelandtime();


            // 修改成员变量类型为 LifecyclePublisher
            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr center_json_sub_;
            std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> cmd_vel_pub_;
            std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> reached_pub_;
            // 定时器
            rclcpp::TimerBase::SharedPtr control_timer_;

            std::string center_json_topic_;
            std::string cmd_vel_topic_;
            std::string reached_topic_;

            // json类实体
            CenterJsonConverter center_coords;
            VelocityJsonConverter vel_cmd;


            // 运动控制相关
            double max_linear_velocity_;
            double y_position_tolerance_;
            double x_position_tolerance_;
            int controlling_period_;
            double max_acceleration_;
            double max_angular_acceleration_;
            std::chrono::duration<double> sampling_duration_;
            double SIGMA_THRESHOLD_;

            // 状态变量
            double current_center_x_;
            double current_center_y_;
            double current_center_z_;
            bool target_detected_;
            ControlState control_state_;
            double avg_center_x_, avg_center_y_, avg_center_z_;
            rclcpp::Time sampling_start_time_;
            
            // 速度控制变量
            double linear_x;
            double linear_y;
            double angular_z;
            double target_linear_x;
            double target_linear_y; 
            double target_angular_z; 
            double prev_linear_x_;
            double prev_linear_y_;
            double prev_angular_z_;
            double movement_time_x_, movement_time_y_;
            double movement_timer_;
            rclcpp::Time movement_start_time;

            // 样本点存储
            std::vector<Point3D> sample_points_;
            Point3D current_center_;
            
            // 添加状态标志
            bool state_is_active_;


            // 运动时间计算相关
            double distance_to_max_vel;
            double time_to_max_vel;
            double constant_vel_time;
            double accel_time;
            double max_distance;

    };

}

#endif